﻿
// 2021/12/2: 增加俯仰角速度和横滚角速度。基于GnssImuSampleV6.cs (版本2021/6/11)
// 2021/12/16: 增加水平误差和垂直误差
// 2022/3/16: 增加行驶曲率
// 2022/10/18: 增加SO标签
// 2022/11/3: 使用SetTime复制全部时间信息
// 2023/5/17: 新增GetArrivalTime方法
// 2023/10/11: 补充部分英文注释
// 2023/11/14: 增加TAI时间类型。增加东北天向速度属性ENUSpeed

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using ASEva;
using SO = System.Reflection.ObfuscationAttribute;

namespace GnssImuSampleV7
{
    [SO]
    enum GnssImuLocationMode
    {
        NoLocation = 0, //无位置信息
        Normal = 1, //默认模式
        RTKFixed = 2, //RTK固定解
        RTKFloat = 3, //RTK浮动解
        RTD = 4, //RTD
        IMUOnly = 5, //仅惯导
        Modified = 6, //后期修正
    }

    [SO]
    enum GnssTimeType
    {
        UTC = 0, // UTC时间
        GPS = 1, // GPS时间
        TAI = 2, // TAI时间
    }

    struct SpeedComponents
    {
        public double east;
        public double north;
        public double up;
    }

    class GnssImuSample : Sample
    {
        public GnssImuLocationMode LocationMode { get; set; } // 位置模式
        public int? SatelliteCount { get; set; } // 卫星数量

        public double? Longitude { get; set; } // [°E] Longitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的经度
        public double? Latitude { get; set; } // [°N] Latitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的纬度
        public double? Altitude { get; set; } // [m] Altitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的海拔

        public double? AntennaLongitude { get; set; } // [°E] Longitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的经度
        public double? AntennaLatitude { get; set; } // [°N] Latitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的纬度
        public double? AntennaAltitude { get; set; } // [m] 天线位置海拔Altitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的海拔

        public double? HorizontalError { get; set; } // [m] 水平位置误差
        public double? VerticalError { get; set; } // [m] 垂直位置误差

        public double? Speed { get; set; } // [kph] 车速

        public double? Orientation { get; set; } // [deg -180~180] 0 as north, CCW is positive / 朝向 CCW为正 北为0
        public double? Pitch { get; set; } // [deg] Heading down is positive / 俯仰角 车头朝下为正
        public double? Roll { get; set; } // [deg] Right side heading down is positive / 横滚角 右侧朝下为正

        public double? YawRate { get; set; } // [deg/s] Turning left is positive / 横摆角速度 左转为正
        public double? PitchRate { get; set; } // [deg/s] Heading down is positive / 俯仰角速度 车头朝下为正
        public double? RollRate { get; set; } // [deg/s] Right side heading down is positive / 横滚角速度 右侧朝下为正

        public double? Ax { get; set; } // [m/s2] Front side is positive / 纵向加速度 朝前为正
        public double? Ay { get; set; } // [m/s2] Left side is positive / 横向加速度 朝左为正
        public double? Az { get; set; } // [m/s2] Top side is positive / 天向加速度 朝上为正

        public double? Curvature { get; set; } // [1/m] Turning left is positive / 行驶曲率 左转为正

        public double? SlipAngle { get; set; } // [deg] Turning left is positive / 侧偏角 左转时为正
        public double? JerkX { get; set; } // [m/s3] Front side is positive / 纵向急动度  朝前为正
        public double? JerkY { get; set; } // [m/s3] Left side is positive / 横向急动度 朝左为正

        public double? ArrivalTime { get; set; } // [s] Arrival time offset (Relative to the session) / 数据到达时间戳（相对Session开始时间）
        public GnssTimeType GnssTimeType { get; set; } // 卫星时间类型
        public DateTime? GnssTime { get; set; } // 卫星时间

        public DateTime? GnssTimeUTC
        {
            get
            {
                if (GnssTime != null)
                {
                    if (GnssTimeType == GnssTimeType.UTC) return GnssTime;
                    else if (GnssTimeType == GnssTimeType.GPS) return GnssTime.Value.AddSeconds(-18);
                    else if (GnssTimeType == GnssTimeType.TAI) return GnssTime.Value.AddSeconds(-37);
                }
                return null;
            }
        }

        public SpeedComponents? ENUSpeed // [kph]
        {
            get
            {
                if (Speed != null && Orientation != null && Pitch != null)
                {
                    var comps = new SpeedComponents();
                    double coef = Math.PI / 180;
                    comps.up = -Speed.Value * Math.Sin(Pitch.Value * coef);
                    double hSpeed = Speed.Value * Math.Cos(Pitch.Value * coef);
                    comps.north = hSpeed * Math.Cos(Orientation.Value * coef);
                    comps.east = -hSpeed * Math.Sin(Orientation.Value * coef);
                    return comps;
                }
                else return null;
            }
        }

        public GnssImuSample()
        { }

        public static string Title
        {
            get
            {
                return "Location mode,Longitude[°],Latitude[°],Altitude[m],Speed[KPH],Orientation[°],Pitch angle[°],Roll angle[°],Yaw rate[°/s],Acceleration-x[m/s2],Acceleration-y[m/s2],Acceleration-z[m/s2],Slip angle[°],Jerk-x[m/s3],Jerk-y[m/s3],Satellite count,Antenna longitude[°],Antenna latitude[°],Antenna altitude[m],GNSS time type,Arrival time[s],GNSS year,GNSS month,GNSS day,GNSS hour,GNSS minute,GNSS second,GNSS millisecond,Pitch rate[°/s],Roll rate[°/s],Horizontal error[m],Vertical error[m],Curvature[1/m],Reserved 34,Reserved 35,Reserved 36,Reserved 37,Reserved 38,Reserved 39,Reserved 40";
		    }
        }

        public static string Protocol
        {
            get
            {
                return "gnssimu-sample-v7";
            }
        }

        public static string[] Protocols
        {
            get
            {
                return new String[] { "gnssimu-sample-v4", "gnssimu-sample-v5", "gnssimu-sample-v6", "gnssimu-sample-v7" };
            }
        }

        public override string GetGeneralSampleProtocol()
        {
            return Protocol;
        }

        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }

        public override GeneralSample ToGeneralSample(int channel)
        {
            var sample = new GeneralSample();
            sample.SetTime(this);
            sample.Protocol = Protocol;
            sample.Channel = channel;

            var data = new GeneralSampleValue[40];
            data[0] = new GeneralSampleValue((double)(int)LocationMode);
            if (Longitude != null) data[1] = new GeneralSampleValue(Longitude.Value);
            if (Latitude != null) data[2] = new GeneralSampleValue(Latitude.Value);
            if (Altitude != null) data[3] = new GeneralSampleValue(Altitude.Value);
            if (Speed != null) data[4] = new GeneralSampleValue(Speed.Value);
            if (Orientation != null) data[5] = new GeneralSampleValue(Orientation.Value);
            if (Pitch != null) data[6] = new GeneralSampleValue(Pitch.Value);
            if (Roll != null) data[7] = new GeneralSampleValue(Roll.Value);
            if (YawRate != null) data[8] = new GeneralSampleValue(YawRate.Value);
            if (Ax != null) data[9] = new GeneralSampleValue(Ax.Value);
            if (Ay != null) data[10] = new GeneralSampleValue(Ay.Value);
            if (Az != null) data[11] = new GeneralSampleValue(Az.Value);
            if (SlipAngle != null) data[12] = new GeneralSampleValue(SlipAngle.Value);
            if (JerkX != null) data[13] = new GeneralSampleValue(JerkX.Value);
            if (JerkY != null) data[14] = new GeneralSampleValue(JerkY.Value);
            if (SatelliteCount != null) data[15] = new GeneralSampleValue(SatelliteCount.Value);
            if (AntennaLongitude != null) data[16] = new GeneralSampleValue(AntennaLongitude.Value);
            if (AntennaLatitude != null) data[17] = new GeneralSampleValue(AntennaLatitude.Value);
            if (AntennaAltitude != null) data[18] = new GeneralSampleValue(AntennaAltitude.Value);
            data[19] = new GeneralSampleValue((double)(int)GnssTimeType);
            if (ArrivalTime != null) data[20] = new GeneralSampleValue(ArrivalTime.Value);
            if (GnssTime != null)
            {
                data[21] = new GeneralSampleValue(GnssTime.Value.Year);
                data[22] = new GeneralSampleValue(GnssTime.Value.Month);
                data[23] = new GeneralSampleValue(GnssTime.Value.Day);
                data[24] = new GeneralSampleValue(GnssTime.Value.Hour);
                data[25] = new GeneralSampleValue(GnssTime.Value.Minute);
                data[26] = new GeneralSampleValue(GnssTime.Value.Second);
                data[27] = new GeneralSampleValue(GnssTime.Value.Millisecond);
            }
            if (PitchRate != null) data[28] = new GeneralSampleValue(PitchRate.Value);
            if (RollRate != null) data[29] = new GeneralSampleValue(RollRate.Value);
            if (HorizontalError != null) data[30] = new GeneralSampleValue(HorizontalError.Value);
            if (VerticalError != null) data[31] = new GeneralSampleValue(VerticalError.Value);
            if (Curvature != null) data[32] = new GeneralSampleValue(Curvature.Value);

            sample.Values = data.ToList();
            sample.NumberOfSignificants = 40;

            return sample;
        }

        public static double? GetArrivalTime(GeneralSample sample)
        {
            switch (sample.Protocol)
            {
                case "gnssimu-sample-v7":
                    if (sample.Values.Count != 40) return null;
                    return sample.Values[20].ToDouble();
                case "gnssimu-sample-v6":
                    if (sample.Values.Count != 28) return null;
                    return sample.Values[20].ToDouble();
                case "gnssimu-sample-v5":
                    if (sample.Values.Count != 19) return null;
                    return sample.Offset;
                case "gnssimu-sample-v4":
                    if (sample.Values.Count != 18) return null;
                    return sample.Offset;
                default:
                    return null;
            }
        }

        public override bool FromGeneralSample(GeneralSample sample)
        {
            SetTime(sample);

            #region gnssimu-sample-v7
            if (sample.Protocol == "gnssimu-sample-v7")
            {
                if (sample.Values.Count != 40) return false;

                LocationMode = sample.Values[0].IsNumber() ? (GnssImuLocationMode)(int)sample.Values[0].number : GnssImuLocationMode.NoLocation;
                Longitude = sample.Values[1].ToDouble();
                Latitude = sample.Values[2].ToDouble();
                Altitude = sample.Values[3].ToDouble();
                Speed = sample.Values[4].ToDouble();
                Orientation = sample.Values[5].ToDouble();
                Pitch = sample.Values[6].ToDouble();
                Roll = sample.Values[7].ToDouble();
                YawRate = sample.Values[8].ToDouble();
                Ax = sample.Values[9].ToDouble();
                Ay = sample.Values[10].ToDouble();
                Az = sample.Values[11].ToDouble();
                SlipAngle = sample.Values[12].ToDouble();
                JerkX = sample.Values[13].ToDouble();
                JerkY = sample.Values[14].ToDouble();
                SatelliteCount = sample.Values[15].ToInt();
                AntennaLongitude = sample.Values[16].ToDouble();
                AntennaLatitude = sample.Values[17].ToDouble();
                AntennaAltitude = sample.Values[18].ToDouble();

                GnssTimeType = sample.Values[19].IsNumber() ? (GnssTimeType)(int)sample.Values[19].ToInt().Value : GnssTimeType.UTC;
                ArrivalTime = sample.Values[20].ToDouble();
                if (sample.Values[21].IsNumber() &&
                    sample.Values[22].IsNumber() &&
                    sample.Values[23].IsNumber() &&
                    sample.Values[24].IsNumber() &&
                    sample.Values[25].IsNumber() &&
                    sample.Values[26].IsNumber() &&
                    sample.Values[27].IsNumber())
                {
                    GnssTime = genDateTime(sample.Values[21].ToInt().Value,
                        sample.Values[22].ToInt().Value,
                        sample.Values[23].ToInt().Value,
                        sample.Values[24].ToInt().Value,
                        sample.Values[25].ToInt().Value,
                        sample.Values[26].ToInt().Value,
                        sample.Values[27].ToInt().Value);
                }
                else
                {
                    GnssTime = null;
                }

                PitchRate = sample.Values[28].ToDouble();
                RollRate = sample.Values[29].ToDouble();
                HorizontalError = sample.Values[30].ToDouble();
                VerticalError = sample.Values[31].ToDouble();
                Curvature = sample.Values[32].ToDouble();

                return true;
            }
            #endregion

            #region gnssimu-sample-v6
            else if (sample.Protocol == "gnssimu-sample-v6")
            {
                if (sample.Values.Count != 28) return false;

                LocationMode = sample.Values[0].IsNumber() ? (GnssImuLocationMode)(int)sample.Values[0].number : GnssImuLocationMode.NoLocation;
                Longitude = sample.Values[1].ToDouble();
                Latitude = sample.Values[2].ToDouble();
                Altitude = sample.Values[3].ToDouble();
                Speed = sample.Values[4].ToDouble();
                Orientation = sample.Values[5].ToDouble();
                Pitch = sample.Values[6].ToDouble();
                Roll = sample.Values[7].ToDouble();
                YawRate = sample.Values[8].ToDouble();
                Ax = sample.Values[9].ToDouble();
                Ay = sample.Values[10].ToDouble();
                Az = sample.Values[11].ToDouble();
                SlipAngle = sample.Values[12].ToDouble();
                JerkX = sample.Values[13].ToDouble();
                JerkY = sample.Values[14].ToDouble();
                SatelliteCount = sample.Values[15].ToInt();
                AntennaLongitude = sample.Values[16].ToDouble();
                AntennaLatitude = sample.Values[17].ToDouble();
                AntennaAltitude = sample.Values[18].ToDouble();

                GnssTimeType = sample.Values[19].IsNumber() ? (GnssTimeType)(int)sample.Values[19].ToInt().Value : GnssTimeType.UTC;
                ArrivalTime = sample.Values[20].ToDouble();
                if (sample.Values[21].IsNumber() &&
                    sample.Values[22].IsNumber() &&
                    sample.Values[23].IsNumber() &&
                    sample.Values[24].IsNumber() &&
                    sample.Values[25].IsNumber() &&
                    sample.Values[26].IsNumber() &&
                    sample.Values[27].IsNumber())
                {
                    GnssTime = genDateTime(sample.Values[21].ToInt().Value,
                        sample.Values[22].ToInt().Value,
                        sample.Values[23].ToInt().Value,
                        sample.Values[24].ToInt().Value,
                        sample.Values[25].ToInt().Value,
                        sample.Values[26].ToInt().Value,
                        sample.Values[27].ToInt().Value);
                }
                else
                {
                    GnssTime = null;
                }

                return true;
            }
            #endregion

            #region gnssimu-sample-v5
            else if (sample.Protocol == "gnssimu-sample-v5")
            {
                if (sample.Values.Count != 19) return false;

                LocationMode = sample.Values[0].IsNumber() ? (GnssImuLocationMode)(int)sample.Values[0].number : GnssImuLocationMode.NoLocation;
                Longitude = sample.Values[1].ToDouble();
                Latitude = sample.Values[2].ToDouble();
                AntennaAltitude = sample.Values[3].ToDouble();
                Speed = sample.Values[4].ToDouble();
                Orientation = sample.Values[5].ToDouble();
                Pitch = sample.Values[6].ToDouble();
                Roll = sample.Values[7].ToDouble();
                YawRate = sample.Values[8].ToDouble();
                Ax = sample.Values[9].ToDouble();
                Ay = sample.Values[10].ToDouble();
                Az = sample.Values[11].ToDouble();

                GnssTimeType = GnssTimeType.UTC;
                ArrivalTime = sample.Offset;
                if (sample.Values[12].IsNumber() &&
                    sample.Values[13].IsNumber() &&
                    sample.Values[14].IsNumber() &&
                    sample.Values[15].IsNumber() &&
                    sample.Values[16].IsNumber() &&
                    sample.Values[17].IsNumber() &&
                    sample.Values[18].IsNumber())
                {
                    GnssTime = genDateTime(sample.Values[12].ToInt().Value,
                        sample.Values[13].ToInt().Value,
                        sample.Values[14].ToInt().Value,
                        sample.Values[15].ToInt().Value,
                        sample.Values[16].ToInt().Value,
                        sample.Values[17].ToInt().Value,
                        sample.Values[18].ToInt().Value);
                }
                else
                {
                    GnssTime = null;
                }

                return true;
            }
            #endregion

            #region gnssimu-sample-v4
            else if (sample.Protocol == "gnssimu-sample-v4")
            {
                if (sample.Values.Count != 18) return false;

                Longitude = sample.Values[0].ToDouble();
                Latitude = sample.Values[1].ToDouble();
                AntennaAltitude = sample.Values[2].ToDouble();
                Speed = sample.Values[3].ToDouble();
                Orientation = sample.Values[4].ToDouble();
                Pitch = sample.Values[5].ToDouble();
                Roll = sample.Values[6].ToDouble();
                YawRate = sample.Values[7].ToDouble();
                Ax = sample.Values[8].ToDouble();
                Ay = sample.Values[9].ToDouble();
                Az = sample.Values[10].ToDouble();

                LocationMode = Longitude != null && Latitude != null ? GnssImuLocationMode.Normal : GnssImuLocationMode.NoLocation;

                GnssTimeType = GnssTimeType.UTC;
                ArrivalTime = sample.Offset;
                if (sample.Values[11].IsNumber() &&
                    sample.Values[12].IsNumber() &&
                    sample.Values[13].IsNumber() &&
                    sample.Values[14].IsNumber() &&
                    sample.Values[15].IsNumber() &&
                    sample.Values[16].IsNumber() &&
                    sample.Values[17].IsNumber())
                {
                    GnssTime = genDateTime(sample.Values[11].ToInt().Value,
                        sample.Values[12].ToInt().Value,
                        sample.Values[13].ToInt().Value,
                        sample.Values[14].ToInt().Value,
                        sample.Values[15].ToInt().Value,
                        sample.Values[16].ToInt().Value,
                        sample.Values[17].ToInt().Value);
                }
                else
                {
                    GnssTime = null;
                }

                return true;
            }
            #endregion

            return false;
        }

        public override bool SupportInterpolation()
        {
            return true;
        }

        protected override Sample Interpolate(Sample.SearchResult input)
        {
            var s1 = input.s1 as GnssImuSample;
            var s2 = input.s2 as GnssImuSample;
            var w1 = input.w1;
            var w2 = input.w2;

            var output = new GnssImuSample();

            if (s1.LocationMode == s2.LocationMode)
            {
                output.LocationMode = s1.LocationMode;
                output.Longitude = s1.Longitude * w1 + s2.Longitude * w2;
                output.Latitude = s1.Latitude * w1 + s2.Latitude * w2;
                output.Altitude = s1.Altitude * w1 + s2.Altitude * w2;
                output.AntennaLongitude = s1.AntennaLongitude * w1 + s2.AntennaLongitude * w2;
                output.AntennaLatitude = s1.AntennaLatitude * w1 + s2.AntennaLatitude * w2;
                output.AntennaAltitude = s1.AntennaAltitude * w1 + s2.AntennaAltitude * w2;
            }
            else
            {
                output.LocationMode = w1 > w2 ? s1.LocationMode : s2.LocationMode;
                output.Longitude = w1 > w2 ? s1.Longitude : s2.Longitude;
                output.Latitude = w1 > w2 ? s1.Latitude : s2.Latitude;
                output.Altitude = w1 > w2 ? s1.Altitude : s2.Altitude;
                output.AntennaLongitude = w1 > w2 ? s1.AntennaLongitude : s2.AntennaLongitude;
                output.AntennaLatitude = w1 > w2 ? s1.AntennaLatitude : s2.AntennaLatitude;
                output.AntennaAltitude = w1 > w2 ? s1.AntennaAltitude : s2.AntennaAltitude;
            }

            output.SatelliteCount = w1 > w2 ? s1.SatelliteCount : s2.SatelliteCount;

            output.Speed = s1.Speed * w1 + s2.Speed * w2;
            output.Orientation = interpolateAngle(s1.Orientation, w1, s2.Orientation, w2);
            output.Pitch = s1.Pitch * w1 + s2.Pitch * w2;
            output.Roll = s1.Roll * w1 + s2.Roll * w2;
            output.YawRate = s1.YawRate * w1 + s2.YawRate * w2;
            output.Ax = s1.Ax * w1 + s2.Ax * w2;
            output.Ay = s1.Ay * w1 + s2.Ay * w2;
            output.Az = s1.Az * w1 + s2.Az * w2;
            output.SlipAngle = s1.SlipAngle * w1 + s2.SlipAngle * w2;
            output.JerkX = s1.JerkX * w1 + s2.JerkX * w2;
            output.JerkY = s1.JerkY * w1 + s2.JerkY * w2;

            output.GnssTimeType = s1.GnssTimeType;
            output.ArrivalTime = s1.ArrivalTime * w1 + s2.ArrivalTime * w2;
            if (s1.GnssTime != null && s2.GnssTime != null)
            {
                var diffSecs = (s2.GnssTime.Value - s1.GnssTime.Value).TotalSeconds;
                output.GnssTime = s1.GnssTime.Value.AddSeconds(diffSecs * w2 / (w1 + w2));
            }

            output.PitchRate = s1.PitchRate * w1 + s2.PitchRate * w2;
            output.RollRate = s1.RollRate * w1 + s2.RollRate * w2;
            output.HorizontalError = s1.HorizontalError * w1 + s2.HorizontalError * w2;
            output.VerticalError = s1.VerticalError * w1 + s2.VerticalError * w2;
            output.Curvature = s1.Curvature * w1 + s2.Curvature * w2;

            return output;
        }

        private double? interpolateAngle(double? a1, double w1, double? a2, double w2)
        {
            if (a1 == null || a2 == null) return null;

            var deg2rad = Math.PI / 180;
            var x1 = Math.Cos(a1.Value * deg2rad);
            var y1 = Math.Sin(a1.Value * deg2rad);
            var x2 = Math.Cos(a2.Value * deg2rad);
            var y2 = Math.Sin(a2.Value * deg2rad);

            var xo = x1 * w1 + x2 * w2;
            var yo = y1 * w1 + y2 * w2;
            if (xo == 0 && yo == 0) return null;

            return Math.Atan2(yo, xo) / deg2rad;
        }

        private DateTime? genDateTime(int year, int month, int day, int hour, int minute, int second, int ms)
        {
            if (year < 2000 || year > 3000) return null;
            if (month < 1 || month > 12) return null;
            if (day < 1 || day > 31) return null;
            if (hour < 0 || hour > 23) return null;
            if (minute < 0 || minute > 59) return null;
            if (second < 0 || second > 59) return null;
            if (ms < 0 || ms > 999) return null;

            try
            {
                return new DateTime(year, month, day, hour, minute, second, ms);
            }
            catch (Exception)
            {
                return null;
            }
        }
    }
}
